/*
 * Modbus slave mode data control.
 * Copyright (c) 2012  everyone
 * All rights reserved.
 * no limite
 */

#define _MBDATACB_MODULE_
#include "mbdatacb.h"
#include "mbinputdatacfg.h"

uint16_t reg_hold_read(uint16_t Index);
uint16_t reg_hold_write(uint16_t Index, uint16_t Val);
uint16_t reg_input_read(uint16_t Index);

/*******************************************************************************
 * @brief  smb_reg_input_cb
 * \param[in] p_reg_buff : data buffer
 * \param[in] reg_addr : data address
 * \param[in] reg_nums : data number
 * \retval:  error st
 *******************************************************************************/
uint8_t smb_reg_input_cb(
    uint8_t *p_reg_buff,
    uint16_t reg_addr,
    uint16_t reg_nums)
{
    uint16_t reg_index;
    mb_data_type dat;

#if REG_INPUT_START == 0
    if ((reg_addr + reg_nums <= REG_INPUT_START + REG_INPUT_NREGS))
    {
#else
    if ((reg_addr >= REG_INPUT_START) && (reg_addr + reg_nums <= REG_INPUT_START + REG_INPUT_NREGS))
    {
#endif
        reg_index = (int)(reg_addr - REG_INPUT_START);
        while (reg_nums > 0)
        {
            dat.Val = reg_input_read(reg_index++);
            *p_reg_buff++ = dat.byte.HB;
            *p_reg_buff++ = dat.byte.LB;
            reg_nums--;
        }
    }
    else
    {
        return MB_ERR_NOREG;
    }
    return MB_ERR_NONE;
}

/*******************************************************************************
 * @brief  smb_reg_holding_cb
 * \param[in] p_reg_buff : data buffer
 * \param[in] reg_addr : data address
 * \param[in] reg_nums : data number
 * \param[in] rw_mode : read or write
 * \retval: error st
 *******************************************************************************/
uint8_t smb_reg_holding_cb(
    uint8_t *p_reg_buff,
    uint16_t reg_addr,
    uint16_t reg_nums,
    mb_reg_mode_type rw_mode)
{
    uint16_t reg_index;
    mb_data_type dat;

#if REG_HOLDING_START == 0
    if ((reg_addr + reg_nums <= REG_HOLDING_START + REG_HOLDING_NREGS))
    {
#else
    if ((reg_addr >= REG_HOLDING_START) && (reg_addr + reg_nums <= REG_HOLDING_START + REG_HOLDING_NREGS))
    {
#endif
        reg_index = (reg_addr - REG_HOLDING_START);
        switch (rw_mode)
        {
        /* Pass current register values to the protocol stack. */
        case MB_REG_READ:
            while (reg_nums > 0)
            {
                dat.Val = reg_hold_read(reg_index++);
                *p_reg_buff++ = dat.byte.HB;
                *p_reg_buff++ = dat.byte.LB;
                reg_nums--;
            }
            break;

        case MB_REG_WRITE:
            while (reg_nums > 0)
            {
                dat.byte.HB = *p_reg_buff++;
                dat.byte.LB = *p_reg_buff++;
                reg_hold_write(reg_index, dat.Val);

                reg_index++;
                reg_nums--;
            }
        }
    }
    else
    {
        return MB_ERR_NOREG;
    }
    return MB_ERR_NONE;
}

/*******************************************************************************
 * @brief  smb_reg_coils_cb
 * \param[in] p_reg_buff : data buffer
 * \param[in] reg_addr : data address
 * \param[in] reg_nums : data number
 * \param[in] rw_mode : read or write
 * \retval: error st
 *******************************************************************************/
uint8_t smb_reg_coils_cb(
    uint8_t *p_reg_buff,
    uint16_t reg_addr,
    uint16_t amount,
    mb_reg_mode_type rw_mode)
{

    mb_data_type dat;
    uint16_t reg_index;
    uint8_t bit_offset;
    uint8_t rd_msk;
    mb_data_type wr_msk;

#if REG_COILS_START == 0
    if ((reg_addr + amount <= REG_COILS_START + REG_COILS_NREGS * 8))
    {
#else
    if ((reg_addr >= REG_COILS_START) && (reg_addr + amount <= REG_COILS_START + REG_COILS_NREGS * 8))
    {
#endif
        bit_offset = reg_addr % 8;
        reg_index = (reg_addr - REG_COILS_START) / 8;

        switch (rw_mode)
        {
        /* Pass current register values to the protocol stack. */
        case MB_REG_READ:
            dat.byte.HB = reg_coil[reg_index++];
            dat.Val >>= (bit_offset);
            while (amount > 0)
            {
                dat.Val >>= (8 - bit_offset);
                dat.byte.HB = reg_coil[reg_index++];
                dat.Val >>= (bit_offset);
                if (amount < 8)
                {
                    rd_msk = 0xFF;
                    dat.byte.LB &= ~(rd_msk << amount);
                    amount = 0;
                }
                else
                {
                    amount -= 8;
                }
                *p_reg_buff++ = dat.byte.LB;
            }
            break;

        /* Update current register values with new values from the
         * protocol stack. */
        case MB_REG_WRITE:
            while (amount > 0)
            {
                dat.byte.LB = *p_reg_buff++;
                dat.Val <<= bit_offset;
                wr_msk.Val = 0xFF;
                if (amount < 8)
                {
                    wr_msk.Val >>= (8 - amount);
                    amount = 0;
                }
                else
                {
                    amount -= 8;
                }
                wr_msk.Val <<= bit_offset;

                dat.Val &= wr_msk.Val;
                reg_coil[reg_index] &= ~wr_msk.byte.LB;
                reg_coil[reg_index + 1] &= ~wr_msk.byte.HB;
                reg_coil[reg_index] |= dat.byte.LB;
                reg_coil[reg_index + 1] |= dat.byte.HB;
                reg_index++;
            }
        }
    }
    else
    {
        return MB_ERR_NOREG;
    }
    return MB_ERR_NONE;
}

/*******************************************************************************
 * @brief  smb_reg_discrete_cb
 * \param[in] p_reg_buff : data buffer
 * \param[in] reg_addr : data address
 * \param[in] reg_nums : data number
 * \retval: error st
 *******************************************************************************/
uint8_t smb_reg_discrete_cb(
    uint8_t *p_reg_buff,
    uint16_t reg_addr,
    uint16_t amount)
{

    mb_data_type dat;
    uint16_t reg_index;
    uint8_t bit_offset;
    uint8_t rd_msk;

#if REG_DISCRETE_START == 0
    if ((reg_addr + amount <= REG_DISCRETE_START + REG_DISCRETE_NREGS * 8))
    {
#else
    if ((reg_addr >= REG_DISCRETE_START) && (reg_addr + amount <= REG_DISCRETE_START + REG_DISCRETE_NREGS * 8))
    {
#endif
        bit_offset = reg_addr % 8;
        reg_index = (reg_addr - REG_DISCRETE_START) / 8;

        dat.byte.HB = reg_discrete[reg_index++];
        dat.Val >>= (bit_offset);
        while (amount > 0)
        {
            dat.Val >>= (8 - bit_offset);
            dat.byte.HB = reg_discrete[reg_index++];
            dat.Val >>= (bit_offset);
            if (amount < 8)
            {
                rd_msk = 0xFF;
                dat.byte.LB &= ~(rd_msk << amount);
                amount = 0;
            }
            else
            {
                amount -= 8;
            }
            *p_reg_buff++ = dat.byte.LB;
        }
    }
    else
    {
        return MB_ERR_NOREG;
    }
    return MB_ERR_NONE;
}
